#include "delay.h"
#include "encoder.h"
#include "led.h"
#include "pid.h"
#include "stm32f10x.h"
#include "stm32f10x_conf.h"
#include "sys.h"
#include "timer.h"
#include "usart.h"

RCC_ClocksTypeDef get_rcc_clock;

int main(void)
{
    u8  angle = 0;
    u8  speed = 0;
    int sign  = 1;
    delay_init();
    PWM_Init();
    Encoder_Init();


#if USART
    uart_init(115200);
#else
    SBUS_USART1_Config(100000);
#endif
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    pid_init(&pid);

    TIM1->CCR4 = 145;
    RCC_GetClocksFreq(&get_rcc_clock);
    while ( 1 ) {
#if USART
        if ( USART_RX_STA & 0x8000 ) {
            if ( USART_RX_BUF[0] == 'R' ) {
                sign = -1;
            }
            else if ( USART_RX_BUF[0] == 'L' ) {
                sign = 1;
            }

            angle      = 100 * (int)(USART_RX_BUF[1] - '0') + 10 * (int)(USART_RX_BUF[2] - '0') + (int)(USART_RX_BUF[3] - '0');
            TIM1->CCR4 = 145 + sign * 25 * angle / 200;


            if ( USART_RX_BUF[4] == 'W' ) {
                sign = 1;
            }
            else if ( USART_RX_BUF[4] == 'S' ) {
                sign = -1;
            }

            speed = 10 * (USART_RX_BUF[5] - '0');

            pid.target = sign * speed;
						

            USART_RX_STA = 0;
        }
        set_motor(pid_count(&pid));
#else
        if ( SBUS_Ch[6] == 200 ) {
            if ( SBUS_Ch[5] == 200 )
                angle = 121;
            else if ( SBUS_Ch[5] == 1000 )
                angle = 145;
            else if ( SBUS_Ch[5] == 1800 )
                angle = 169;
        }
        else if ( SBUS_Ch[6] == 1800 ) {
            if ( SBUS_Ch[5] == 200 )
                angle = 129;
            else if ( SBUS_Ch[5] == 1000 )
                angle = 145;
            else if ( SBUS_Ch[5] == 1800 )
                angle = 161;
        }
        TIM1->CCR4 = angle;

        speed      = (SBUS_Ch[3] - 200) / 32;
        pid.target = speed;
        if ( SBUS_Ch[2] < 500 ) {
            pid.target = -1 * pid.target;
        }
        pid.output = pid_count(&pid);
        set_motor(pid.output);

#endif
        delay_ms(10);
    }
}
